#pragma once
#include "port/CanPort.hpp"
// #include "port/SerialPort.hpp"
#include <bits/stdint-uintn.h>
#include <queue>

#define Pi 3.141592653

namespace wmj
{
    class WMJRobotControl
    {
    public:
        
        ROBO_STATE m_cur_robo_state{STATE_ARMOR};
        CAMERA_EXPOSURE m_armor_camera_exposure{EXPOSURE_MIDDLE};
        CAMERA_EXPOSURE m_dark_camera_exposure{EXPOSURE_HIGH};
        bool m_aim_armor = false;
        bool m_enable_shoot = false;
        bool m_auto_shoot = true;
        bool m_use_number_detect = true;

        bool m_shoot_speed_initialized = false;
        ROBO_SHOOT m_shoot_level = wmj::ROBO_SHOOT::SPEED_IDLE;
        std::deque<float> m_prev_shoot_speed;
        float m_cur_shoot_speed = 0;
        float m_fifteen_shoot_speed;
        float m_eighteen_shoot_speed;
        float m_thirty_shoot_speed;

        WMJRobotControl();
        ~WMJRobotControl(){};

        // 陀螺仪通信
        std::pair<int, cv::Point3d> getGyroAngle();
        std::pair<int, cv::Point3d> getGyroVelocity();
        std::pair<int, cv::Point3d> getGyroAcce();
        std::vector<std::pair<int, cv::Point3d>> GetGyroInfo();

    public:
        // 存储从电控接收到的数据包解析出的IMU原始数据和解析日志
        IMURAW_LOG ImuRaw_LOG;
        /**
         * @brief
         * IMU通信。
         * 返回IMU原始数据和数据处理日志。
         *
         * @return IMURAW_LOG
         */
        IMURAW_LOG getImuRaw();

        // 底盘通讯
        void brake();
        void setTopAndSpeed(bool power, bool use_cap, float x_speed, float y_speed);
        void setChassisSpeed(float x_speed, float y_speed);
        void top(bool power);
        std::vector<float> getSpeed();
        bool get_top();
        int test = 2;
        // 云台通讯
        void SwitchBaseCoor(bool Mode);
        void SetGimbalAngle(float pitch_angle, float yaw_angle, GIMBAL_ORIENTATION orientation);
        void SetGimbalSpeed(float pitch_speed, float yaw_speed, GIMBAL_ORIENTATION orientation);
        void SetGimbal_YawSpeed_PitchAngle(float pitch_angle, float yaw_speed, GIMBAL_ORIENTATION orientation);
        GimbalPose GetGimbalAngle(GIMBAL_ORIENTATION orientation);
        float GetBulletSpeed(GIMBAL_ORIENTATION orientation = LEFT);
        int GetGimbalAngle(GimbalPose &);
        GimbalPose GetGimbalSpeed(GIMBAL_ORIENTATION orientation);
        // 新加的，包括获取云台在地面系和车体系下的角速度或角度
        GimbalPose getGimbalPoseAngle(GIMBAL_ORIENTATION orientation);
        GimbalPose getGimbalPoseSpeed(GIMBAL_ORIENTATION orientation);
        std::vector<float> getGimbalRotationRange(GIMBAL_ORIENTATION orientation);
        std::vector<float> getBigYaw();
        std::vector<float> getBigYaw2();
        void setBigYaw(uint8_t big_yaw_flag, float value);
        void setSentryCmd(bool comfirm_respawn, bool apply_instant_respawn,int apply_bullet_redeem_num, int apply_remote_supply_times, int apply_remote_heal_times);

        // 射击通讯
        int GetBulletNumber();
        void openBox();
        void ShootNone(GIMBAL_ORIENTATION orientation);
        void ShootOnce(GIMBAL_ORIENTATION orientation);
        void ShootSome(int BulletNum, GIMBAL_ORIENTATION orientation);
        void KeepShoot(GIMBAL_ORIENTATION orientation);
        void StopShoot(GIMBAL_ORIENTATION orientation);

        // 视觉信息通讯
        void SendVisionCurrentStatue(ROBO_STATE Mode, CAMERA_EXPOSURE exposure, bool auto_shoot_ok, int id,
                                     bool ifrecognize, bool light_ok, bool light_compare_ok, bool number_ok,
                                     int cameranum, int erroritem, bool armor_compare_ok, bool select_ok);
        void CameraTrigger(bool v_trigger_enable, int v_rate, int v_delay_time);
        GameStateMsg getGameState();

        // 主控通讯
        bool sendInfo(uint8_t, uint8_t, bool, float);
        bool HighShootSpeed();
        wmj::KeyboardClick m_click;
        KeyboardInfo GetKeyboardInfo();
        ROBO_STATE GetRobotStatus();
        std::string GetCameraExposure();
        GimbalPose GetAdjustPose();
        ROBO_STATE LastState;

        // 裁判系统绘图
        void drawCircle(ROBO_LAYER layer, ROBO_COLOR color, cv::Point center_point, int radius);
        void drawLine(ROBO_LAYER layer, ROBO_COLOR color, cv::Point center_point, cv::Point end_point);
        void drawRect(ROBO_LAYER layer, ROBO_COLOR color, cv::Point center_point, cv::Point diag_point);
        void drawOval(ROBO_LAYER layer, ROBO_COLOR color, cv::Point center_point, cv::Point width_height);
        void clearAll();
        void clearLayer(ROBO_LAYER layer);

        // 裁判系统接收
        // ROBO_GAME getGameState();
        ROBO_ENERGY getEnergyStatus();
        float getShootSpeedValue();
        float getMaxShootSpeed();
        bool getVirtualShield();
        ROBO_GAIN getRobotGain();
        ROBO_RFID getRobotRFID();
        int getLeftHP();
        int getBaseHP();
        int getOutpostHP();

        // 机器人通讯
        uint8_t recvFromRobot();
        void sendToRobot(ROBO_ID, uint8_t test);

    private:
        enum
        {
            ChassisCoor,
            GroundCoor
        };
        std::shared_ptr<wmj::Port> canBus;
        std::shared_ptr<wmj::Port> canBus_spare;
        std::shared_ptr<wmj::Port> gyroCanBus;
        uint8_t _BaseCoor{ChassisCoor * 0x40};
        wmj::GimbalPose lastpose;

        bool m_box_open = false;
        bool m_keep_shoot = false;
        bool m_stop_shoot_compulsive = false;
        bool poseInit;
        int m_port_type;
        int circlecnt;
        bool m_curTopStatus;

        GimbalPose m_adjust_pose = GimbalPose(0, 0);
        float last_gimal_time = 0;

        void setGimbal(uint8_t mode, float pitch_angle, float yaw_angle, float pitch_speed, float yaw_speed, GIMBAL_ORIENTATION orientation);
        void setChassis(uint8_t info1, float x_speed, float y_speed);
        std::vector<float> getGimbal(GIMBAL_ORIENTATION orientation);
        

        void sendFrame(Buffer &data);
        Buffer readFrame(int flag);

        void setNavigationChassis(uint8_t info1, uint8_t func, float x_speed, float y_speed, float target_angle);
        void drawShape(ROBO_OPT opt, ROBO_SHAPE shape, ROBO_LAYER layer, ROBO_COLOR color, cv::Point start_point, cv::Point end_point, ROBO_DELETE del = DELETE_NONE);
        void checkCLickAndSetStatus(bool flag, bool &click, bool &status);
    };
}
